/*
	aco-netdesign -- Ant colony optimization solution to network design problem

	Copyright (C) 2008 Jeffrey Sharkey, http://jsharkey.org/
	
	Developed by Jeffrey Sharkey as part of his thesis work at Montana State
	University. His work was sponsored by the Western Transportation Institute,
	and was guided by advisor Doug Galarus. Other valuable guidance was
	provided by Dr. Bill Jameson and Gary Schoep. 

	This program is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.
	
	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.
	
	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "aco.h"
#include "graph.h"


double SolverACO::minPher = 1;
double SolverACO::defPher = 10;
double SolverACO::maxPher = 20;
double SolverACO::increment = 1;

double SolverACO::alphaHeur = 1;
double SolverACO::alphaPher = 1;
double SolverACO::alphaRand = 1;
double SolverACO::alphaPref = 1;




Ant::Ant(SolutionACO* s_, Vertex* start_) : Path(start_, NULL) {
	s = s_;
	forceEndgame = false;
	end = s->g->root;
	bandwidth = start_->bandwidth;
}

Ant::~Ant() {
}

bool Ant::finished() {
	//return (*current == *end);
	return (forceEndgame || *current == *end);
}

double Ant::localHeuristic(Edge* e) {
	Vertex* consider = e->adjacent(current);
	
	// check to see if considered vertex is in tabu list
	if(vertexes.contains(consider)) {
		//if(!s->silent) cout << "======== TABU! ========" << endl;
		return IS_TABU;
	}
	
	// dont allow us to pass through terminal vertexes unless root
	if(!consider->root && consider->terminal)
		return IS_TABU;
	
	// check if there is enough bandwidth available
	if(bandwidth > e->bandwidth)
		return IS_TABU;
	
	// if this vertex is already part of solution, endgame it
	if(s->vertexes.contains(consider))
		return ENDGAME;
	
	// decide best outcome of following this edge
	double lowestCost = s->look->get(consider, s->g->root);

// simplify the heuristic by only working towards root node
/** /
	ulist<Vertex*>::iterator it;
	for(it = s->vertexes.begin(); it != s->vertexes.end(); it++) {
		// skip if already in this path
		Vertex* tree = (Vertex*)(*it);
		if(vertexes.contains(tree)) continue;
		lowestCost = min(lowestCost, s->look->get(consider, tree));

	}
/**/
	
	//if(!s->silent) cout << lowestCost << endl;

	
double theur = pow(1 / (lowestCost + 1), SolverACO::alphaHeur);
double tpref = pow(consider->pref, SolverACO::alphaPref);
double tpher = pow(s->pheromones[consider] / 20, SolverACO::alphaPher);
//double trand = pow(((double)rand() / (double)RAND_MAX), SolverACO::alphaRand);
double trand = pow(mt_rand(), SolverACO::alphaRand);

//cout << "h=" << theur << "\tp=" << tpher << "\tr=" << trand << endl;

double mixed = theur * tpher * trand * tpref;

//	heur *= pow((double)1 / (lowestCost + (double)1), SolverACO::alphaHeur);
//	heur *= pow((double)s->pheromones[consider] / (double)20, SolverACO::alphaPher);
//	heur *= pow(((double)rand() / (double)RAND_MAX), SolverACO::alphaRand);

//cout << heur << endl;
	return mixed;
}







double Ant::mixedValue(Edge* e, SolutionACO* s) {
	Vertex* consider = e->adjacent(current);
	
	// check to see if considered vertex is in tabu list
	if(vertexes.contains(consider)) {
//cout << "IS_TABU: already visted" << endl; cout.flush();
		return IS_TABU;
	}
	
	// dont allow us to pass through terminal vertexes unless root
	if(consider->root) {
//cout << "\t\tCONSIDERING ROOT YOU NOOB!" << endl; cout.flush();
		return ENDGAME;
	} else if(consider->terminal) {
//cout << "IS_TABU: tryign to visit terminal" << endl; cout.flush();
		return IS_TABU;
	}

	// edges returning to a lower level are also tabu
//	if(consider->layer > current->layer) {
//cout << "IS_TABU: trying to go to lower layer" endl; cout.flush();
//		return IS_TABU;
//	}
	
	// check if there is enough bandwidth available
	if(bandwidth > s->bandwidth[e]) {
//cout << "IS_TABU: bandwidth overload" << endl; cout.flush();
		return IS_TABU;
	}

	// when coming from the terminal layer, bias towards nearby nodes
//	double slightBias = 1;
//	if(current->terminal) {
//		slightBias = 1/abs(current->id - consider->id);
//	}
	
	// find shortest path from considered node to root
	double lowestCost = s->look->get(consider, s->g->root);
	bool fromtrail = s->vertexes.contains(current);
	bool totrail = s->vertexes.contains(consider);

if(totrail) {
	return ENDGAME;
}

	if(fromtrail && totrail) {
	} else if(fromtrail && !totrail) {
		// penalize trails that lead away from existing ant trail
		lowestCost *= 2;
	} else {
		lowestCost -= 0.1;
		ulist<Vertex*>::iterator it;
		for(it = s->vertexes.begin(); it != s->vertexes.end(); it++) {
			// skip if already in this path
			Vertex* tree = (Vertex*)(*it);
			if(vertexes.contains(tree)) continue;
			lowestCost = min(lowestCost, s->look->get(consider, tree));
		}
	}


	double pher = pow(s->pheromones[consider] / 20, SolverACO::alphaPher);
	double heur = pow(1 / (lowestCost + 1), SolverACO::alphaHeur);
	//double bias = pow(slightBias, 1);

	return pher * heur;
}








bool randomSort(Ant* a, Ant* b) {
//	return (((double)rand() / (double)RAND_MAX) > 0.5);
	return (mt_rand() > 0.5);
}


SolutionACO::SolutionACO(Graph* g_, Lookup* look_) : g(g_), look(look_) {
	silent = false;
	
	// create ant for each terminal vertex, but not the root

// keep a local bandwidth record for each edge
map<int,Edge*>::iterator j;
for(j = g->edges.begin(); j != g->edges.end(); j++) {
	Edge* e = (*j).second;
	bandwidth[e] = e->bandwidth;
}



//cout << "SolutionACO: creating list of all ants" << endl; cout.flush();
	ulist<Vertex*>::iterator it;
	for(it = g->terminal.begin(); it != g->terminal.end(); it++) {
		Vertex* v = (Vertex*)(*it);
		if(v->root) continue;
		Ant* ant = new Ant(this, v);
		ant->bandwidth = v->bandwidth;
//cout << "assigning ant->bandwidth=" << ant->bandwidth << endl;
		ants.insert(ant);
	}
	
	// randomly shuffle list of ants and make copy
//cout << "SolutionACO: shuffling ants" << endl; cout.flush();
	ants.sort(randomSort);
	unsolved = ants;

/* check to see random order * /
ulist<Ant*>::iterator j;
int k = 0;
for(j = ants.begin(); j != ants.end(); j++) {
	Ant* a = *j;
	cout << a->start->id << "\t";
	if(k++ > 7) break;
}
cout << endl;
cout.flush();
/**/
	
	t = new TimeRemaining(ants.size(), 32);
	t->silent = true;
	
	// TODO: read in and set default available bandwidths
	
}

SolutionACO::~SolutionACO() {
	ulist<Ant*>::iterator it;
	for(it = ants.begin(); it != ants.end(); it++) {
		Ant* a = *it;
		delete a;
	}
	delete t;
}

double SolutionACO::globalHeuristic() {
	double heur = 1;
	heur /= cost();
	return heur;
}

void SolutionACO::solve() {
	while(!solved())
		iteration();
}

bool SolutionACO::solved() {
	if(invalid) return true;
	return (unsolved.size() == 0);
}

void SolutionACO::iteration() {
	// choose a single ant and iterate them through the graph
	assert(unsolved.size() > 0);
	Ant* ant = unsolved.front();
	unsolved.erase(unsolved.begin());
	
	while(!ant->finished()) {
		// choose best possible edge to follow
		ulist<Edge*> adjacent = g->adjacent(ant->current);
//cout << "iteration() considering |E|=" << adjacent.size() << endl;


// newer mixing method

Edge* best = NULL;
map<Edge*,double> interval;
bool endgame = false;

// walk through all possible edges collecting wheel sizes
ulist<Edge*>::iterator it;
double end = 0;
for(it = adjacent.begin(); it != adjacent.end(); it++) {
	Edge* consider = *it;
	double mixed = ant->mixedValue(consider, this);

	// ignore tabu edges and handle endgame edges
	if(mixed == IS_TABU) continue;
	if(mixed == ENDGAME) {
		best = consider;
		endgame = true;
		break;
	}

//cout << "slice=" << mixed << endl;

	// keep track of ending interval of this edge on the randomness wheel
	end += mixed;
	interval[consider] = end;
}

//cout << endl << "----------------------------------" << endl;

if(!endgame) {
	// generate random number on interval between 0 and end
	double spin = mt_rand() * end;

	// find the winning edge
	map<Edge*,double>::iterator j;
	for(j = interval.begin(); j != interval.end(); j++) {
		best = (*j).first;
		double thisend = (*j).second;

		// jump out of sequence when we find interval we landed in
		if(spin < thisend) break;
	}

}





/* older method		
		Edge* best = NULL;
		double bestScore = SMALL_COST;
		
		ulist<Edge*>::iterator it;
		for(it = adjacent.begin(); it != adjacent.end(); it++) {
			Edge* consider = (Edge*)(*it);
			double score = ant->localHeuristic(consider);
			if(score > bestScore) {
				best = consider;
				bestScore = score;
			}
		}

		assert(best != NULL);
		
		// handle tabu and endgame nodes, following edges as needed
		if(bestScore == IS_TABU) {
			invalid = true;
			return;
		} else {
			ant->follow(best);
			if(bestScore == ENDGAME)
				ant->forceEndgame = true;
		}
*/

// handle case where all tabu or no winner found
if(best == NULL) {
	invalid = true;
	return;
}

ant->follow(best);
bandwidth[best] -= ant->bandwidth;
//cout << "following to vertex " << ant->current->id << " on layer " << ant->current->layer << endl;
//cout << "reducing bandwidth on followed link, now is " << bandwidth[best] << endl;
if(endgame) {
	ant->forceEndgame = true;
}


		
		
	}
	
	// merge the solved ant path with this current solution
	merge((Path*) ant);
	t->increment();
	
//cout << "|"; cout.flush();

}


SolverACO::SolverACO(Graph* g_, Lookup* look_) : Solver(g_, look_) {
	silent = false;
	map<int,Vertex*>::iterator it;
	for(it = g->vertexes.begin(); it != g->vertexes.end(); it++) {
		Vertex* v = (*it).second;
		pheromones[v] = defPher;
	}
}

SolutionACO* SolverACO::fullSolve(int generations, int population) {
	// perform a full solve including pheromone updates
	SolutionACO* overallBest = NULL;
	double overallBestScore = 0;
	int lastchanged = 0;
	
	for(int i = 0; i < generations; i++) {
		ulist<SolutionACO*> pile = buildGeneration(population);
		if(pile.empty()) continue;
		
		SolutionACO* best = NULL;
		double bestScore = 0;
		
		ulist<SolutionACO*>::iterator it;
		for(it = pile.begin(); it != pile.end(); it++) {
			SolutionACO* s = (*it);
			double score = s->globalHeuristic();
			if(score > bestScore) {
				delete best;
				best = s;
				bestScore = score;
			} else {
				delete s;
			}
		}
		
		// update pheromones with the best solution in this round
		assert(best != NULL);
		updatePheromones(best);
		
		if(bestScore > overallBestScore) {

double diff = (1 / bestScore) - (1 / overallBestScore);
if(diff > 0.5 || diff < -0.5) {
	lastchanged = i;
}

			delete overallBest;
			overallBest = best;
			overallBestScore = bestScore;
		} else {
			delete best;
		}
		
if(!this->silent) {
	cout << " overallBest.cost=" << overallBest->cost() << endl;
}

	}
	

//cout << endl << "==== aco.generations()=" << lastchanged;
	this->generations = lastchanged;
	assert(overallBest != NULL);
	return overallBest;
}



#include <pthread.h>

#define THREADS 8

SolverACO* solver;
//ulist<SolutionACO*>* pile = new ulist<SolutionACO*>();
SolutionACO** pile;
int pilei, remaining;

SolutionACO* thread_best;
double thread_score;

bool resetnextid;

pthread_mutex_t mutexpile = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mutexremain = PTHREAD_MUTEX_INITIALIZER;


void *thread_func_solver(void *vptr_args) {


while(true) {


pthread_mutex_lock(&mutexremain);
--remaining;
if(remaining < 0) {
	pthread_mutex_unlock(&mutexremain);
	return NULL;
}
pthread_mutex_unlock(&mutexremain);



SolutionACO* s = NULL;
do {
	delete s;

	// check to see if we are close to the maxid
	if(Tagged::nextid > NEARMAXID) {
		// set reset flag, return job we were assigned to, and merge back
		resetnextid = true;
		pthread_mutex_lock(&mutexremain);
		++remaining;
		pthread_mutex_unlock(&mutexremain);
		return NULL;
	}

	s = solver->singleSolution();
	if(!solver->silent && s->invalid) {
		cout << "]"; cout.flush();
	}
} while(s->invalid);

double score = s->globalHeuristic();


pthread_mutex_lock(&mutexpile);

if(score > thread_score) {
	thread_score = score;
	delete thread_best;
	thread_best = s;
} else {
	delete s;
}

//pile[pilei++] = s;


//pile->insert(s);
//cout << pile->size();

pthread_mutex_unlock(&mutexpile);


if(!solver->silent) {
	cout << "."; cout.flush();
}

}

return NULL;
	
}





ulist<SolutionACO*> SolverACO::buildGeneration(int population) {

/**/

//pile->clear();
pile = new SolutionACO*[population];
pilei = 0;
remaining = population;

thread_best = NULL;
thread_score = 0;

resetnextid = false;

solver = this;

// reset id counter in tagged
Tagged::nextid = 0;


while(true) {

pthread_t thread[THREADS];
//cout << "buildGeneration: creating threads" << endl;
for(int i = 0; i < THREADS; i++) {
	pthread_create(&thread[i], NULL, &thread_func_solver, NULL);
}
	
	
//cout << "buildGeneration: joining threads" << endl;
for(int i = 0; i < THREADS; i++) {
	pthread_join(thread[i], NULL);
}


// if weve had problems with maxtag, then reset and respawn threads
if(resetnextid) {
	Tagged::nextid = 0;
	cout << "$"; cout.flush();
	resetnextid = false;
	continue;
}

break;

}


// merge all into one nice stl list
ulist<SolutionACO*> pileout;

pileout.insert(thread_best);
/*
for(int i = 0; i < pilei; i++) {
	pileout.insert(pile[i]);
}
delete[] pile;
*/


//cout << "buildGeneration: threads done, generated " << pileout.size() << endl;

//ulist<SolutionACO*>::iterator it;
//for(it = pile->begin(); it != pile->end(); it++) {
//	SolutionACO* s = (SolutionACO*)(*it);
//	delete s;
//}
//pile->clear();


return pileout;
/**/

/** /
	// build a generation of a population of solutions
	ulist<SolutionACO*> pile;
	for(int i = 0; i < population; i++) {
		SolutionACO* s = singleSolution();
		if(s->invalid) {
			delete s;
			continue;
		}
		pile.insert(s);
		cout << "."; fflush(stdout);
	}
	return pile;
/**/
}

void SolverACO::updatePheromones(SolutionACO* s) {

	map<Vertex*, double>::iterator it;
	for(it = pheromones.begin(); it != pheromones.end(); it++) {
		Vertex* v = (Vertex*)(*it).first;
		double value = (*it).second;

		if(s->vertexes.contains(v)) {
			// increment higher for included notes
			value += increment;
		} else {
			// decay pheromone value for everyone else
			value *= 0.9;
		}

		// force value within bounds and assign it
		value = min(max(value, minPher), maxPher);
		(*it).second = value;
	}


/* older method of static updates
	map<Vertex*, double>::iterator it;
	for(it = pheromones.begin(); it != pheromones.end(); it++) {
		Vertex* v = (Vertex*)(*it).first;
		double value = (*it).second;
		value += s->vertexes.contains(v) ? +increment : -increment;
		value = min(max(value, minPher), maxPher);
		(*it).second = value;
	}
*/
}

SolutionACO* SolverACO::singleSolution() {
	// build a single solution with current pheromones
	SolutionACO* s = new SolutionACO(g, look);
	s->silent = silent;
	s->pheromones = pheromones;
	s->solve();
	//cout << "cost()=" << s->cost() << endl;
	return s;
}

void SolverACO::dumpPheromones() {
	// dump out all pheromone information
	map<Vertex*, double>::iterator it;
	for(it = pheromones.begin(); it != pheromones.end(); it++) {
		Vertex* v = (Vertex*)(*it).first;
		double value = (*it).second;
		if(v->terminal) continue;
		cout << v->id << "=" << value << "\t";
	}
}


